//
// = FILENAME
//    Serializer.c
//
// = AUTHOR(S)
//    Patric Jensfelt
//    
// = COPYRIGHT
//    Copyright (c) 2010 Patric Jensfelt
//
/*----------------------------------------------------------------------*/

#include <unistd.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <cstdio>
#include <cmath>
#include <cstring>
#include <ctype.h>
#include <iostream>
#include <sstream>

#include "Serializer.hh"

Serializer::Serializer()
  :m_Fd(-1)
{
  m_RecvBufSize = 1024;
  m_RecvBuf = new char[m_RecvBufSize];
  m_RecvBufPos = 0;

  m_ReplyTimeout = 0.3;

  m_MaxIO = 3;
}

Serializer::~Serializer()
{
  disconnect();

  if (m_RecvBuf) {
    delete [] m_RecvBuf;
    m_RecvBuf = 0;
  }
}

bool 
Serializer::connect(const std::string &device, int baudrate)
{
  m_Fd = open(device.c_str(), O_RDWR | O_NONBLOCK);
  
  if (m_Fd < 0) {
    std::cerr << "Serializer::connect Failed to open serial device \""
	      << device << "\"\n";
    return false;
  }

  if (tcgetattr(m_Fd, &m_OrigTermios) < 0) { 
    perror("Error using TCGETS in ioctl ");
    return false;
  }

  // Flush both in and out buffers
  if (tcflush(m_Fd, TCIOFLUSH)) {
    perror("Error flushing i/o buffers\n");
    return false;
  }

  if (setTermios(baudrate)) {
    std::cerr << "Serializer::connect Failed to set termios\n";
    close(m_Fd);
    m_Fd = -1;
    return false;
  }

  if (handshake()) {
    std::cerr << "Serializer::connect Failed on " << device 
	      << " at " << baudrate << " baud\n";
    std::cerr << "NOTE:\n"
              << "NOTE: The default baudrate for the Serializer is 19200\n"
              << "NOTE: You need to make sure you use the write baudrate\n"
              << "NOTE:\n";
    return false;
  }
  
  std::cout << "Serializer::connect Success on "
	    << device << " at " << baudrate << std::endl;
  return true;
}

void 
Serializer::disconnect()
{
  stopAllMotors();
  std::vector< std::pair<int,bool> > cmd;
  cmd.resize(m_MaxIO + 1);
  for (unsigned int i = 0; i < cmd.size(); i++) {
    cmd[i].first = i;
    cmd[i].second = false;
  }
  setIO(cmd);

  if (m_Fd <= 0) return;

  // Restore old termios settings, no need to do error checking
  // becuase we will not be able to doanything about it
  tcsetattr(m_Fd, TCSANOW, &m_OrigTermios);

  // Close the device
  close(m_Fd);

  // Make sure to let the rest of the system know that the device is closed
  m_Fd = -1;
}

bool
Serializer::restoreToFactoryDefaults()
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::reset NOT CONNECTED\n";
    return false;
  }

  char msg[]="restore\r";
  sendMsg(msg);
  emptyRecvBuf();
  blockingRecvReply(1);
  return true;
}

void 
Serializer::setMaxIOindex(int maxIOindex)
{
  if (maxIOindex < 0) 
    m_MaxIO = -1;
  else if (maxIOindex > 9) {
    m_MaxIO = 9;
    std::cerr << "Serializer::setMaxIOindex limiting max index to 9 from "
	      << maxIOindex << "\n";
  } else {
    m_MaxIO = maxIOindex;
  }
  
}

bool 
Serializer::resetBoard()
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::reset NOT CONNECTED\n";
    return false;
  }

  char msg[]="reset\r";
  sendMsg(msg);

  emptyRecvBuf();
  if (blockingRecvReply(3)) return false;
  std::cerr << "Reset board resulted in \""
	    << m_RecvBuf << "\"\n";
  return true;
}

bool
Serializer::setBaudrate(int baudrate)
{
  char msg[64];

  switch (baudrate) {
  case 2400:
    sprintf(msg, "cfg baud 0\r");
    break;
  case 4800:
    sprintf(msg, "cfg baud 1\r");
    break;
  case 9600:
    sprintf(msg, "cfg baud 2\r");
    break;
  case 19200:
    sprintf(msg, "cfg baud 3\r");
    break;
  case 57600:
    sprintf(msg, "cfg baud 4\r");
    break;
  case 115200:
    sprintf(msg, "cfg baud 5\r");
    break;
  default:
    std::cerr << "Serializer::setBaudrate Only supporting rates "
              << "2400,4800,9600,19200,57600 and 115200, "
              << " not " << baudrate << "\n";
    return false;
  }

  sendMsg(msg);
  return (waitForACK(1));
}

bool
Serializer::setUnit(enum UnitType u)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setUnit NOT CONNECTED\n";
    return false;
  }

  char msg[64];
  sprintf(msg, "cfg units %d\r",int(u));
  sendMsg(msg);
  if (waitForACK(1)==0) return true;

  std::cerr << "Serializer::setUnit(" << int(u) << " FAILED\n";
  return false;
}

bool 
Serializer::setLEDFreq(int freq)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setLEDFreq NOT CONNECTED\n";
    return false;
  }

  if (freq < 1) freq = 1;
  if (freq > 127) freq = 127;

  char msg[128];
  sprintf(msg, "blink 1:%d\r", freq);
  sendMsg(msg);
  
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setLEDFreq FAILED\n";
  return false;
}

bool
Serializer::setEncoderType(enum EncoderType e)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setEncoderType NOT CONNECTED\n";
    return false;
  }

  char msg[128];
  if (e == ENCODER_SINGLE) {
    std::cerr << "Configuring for SINGLE channel encoders\n";
    sprintf(msg, "cfg enc 0\r");
  } else {
    std::cerr << "Configuring for QUAD encoders\n";
    sprintf(msg, "cfg enc 1\r");
  }
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setEncoderType FAILED\n";
  return false;
}

bool 
Serializer::getEncoder(int channel, long &enc)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getEncoder NOT CONNECTED\n";
    return false;
  }

  char msg[128];

  if (channel != 0 && channel != 1) {
    std::cerr << "Serializer::getEncoder channel="
              << channel << " not supported, only 0 and 1\n";
    return false;
  }

  if (channel == 0) {
    sprintf(msg, "getenc 1\r");
  } else {
    sprintf(msg, "getenc 2\r");
  }
  sendMsg(msg);
  
  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getEncoder(" << channel << ") FAILED (no data)\n";
    return false;
  }

  std::istringstream str(m_RecvBuf);
  if ( !(str >> enc) ) {
    std::cerr << "Serializer::getEncoder(" << channel << ") FAILED (parse error)\n";
    return false;
  }

  return true;
}

bool 
Serializer::getEncoders(long &enc0, long &enc1)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getEncoders NOT CONNECTED\n";
    return false;
  }

  sendMsg("getenc 1 2\r");

  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getEncoders FAILED (no data!)\n";
    return false;
  }

  std::istringstream str(m_RecvBuf);
  if ( !(str >> enc0 >> enc1) ) {
    std::cerr << "Serializer::getEncoders FAILED (parse error)\n";
    return false;
  }

  return true;
}

bool 
Serializer::clearEncoder(int channel)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::clearEncoder NOT CONNECTED\n";
    return false;
  }

  char msg[128];

  if (channel != 0 && channel != 1) {
    std::cerr << "Serializer::clearEncoder channel="
              << channel << " not supported, only 0 and 1\n";
    return false;
  }

  if (channel == 0) {
    sprintf(msg, "clrenc 1\r");
  } else {
    sprintf(msg, "clrenc 2\r");
  }
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::clearEncoder(" << channel << ") FAILED\n";
  return false;
}

bool 
Serializer::getVPID(int &P, int &I, int &D, int &L)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getVPID NOT CONNECTED\n";
    return false;
  }

  sendMsg("vpid\r");
  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getVPID FAILED (no data)\n";
    return false;
  }

  std::cerr << m_RecvBuf << std::endl;

  if (sscanf(m_RecvBuf, "P:%d I:%d D:%d L:%d", &P, &I, &D, &L) != 4) {
    std::cerr << "Serializer::getVPID FAILED (parse error)\n";
    return false;
  }

  return true;
}

bool 
Serializer::setVPID(int P, int I, int D, int L)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setVPID NOT CONNECTED\n";
    return false;
  }

  char msg[128];
  sprintf(msg, "vpid %d:%d:%d:%d\r", P,I,D,L);
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setVPID FAILED\n";
  return false;
}

bool 
Serializer::getDPID(int &P, int &I, int &D, int &A)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getDPID NOT CONNECTED\n";
    return false;
  }

  sendMsg("dpid\r");
  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getVPID FAILED (no data)\n";
    return false;
  }

  std::cerr << m_RecvBuf << std::endl;

  if (sscanf(m_RecvBuf, "P:%d I:%d D:%d A:%d", &P, &I, &D, &A) != 4) {
    std::cerr << "Serializer::getDPID FAILED (parse error)\n";
    return false;
  }

  return true;
}

bool 
Serializer::setDPID(int P, int I, int D, int A)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setDPID NOT CONNECTED\n";
    return false;
  }

  char msg[128];
  sprintf(msg, "dpid %d:%d:%d:%d\r", P,I,D,A);
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setDPID FAILED\n";
  return false;
}

bool
Serializer::getDIGOstate(bool &busy)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getDIGOstate NOT CONNECTED\n";
    return false;
  }

  sendMsg("pids\r");

  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getDIGOstate FAILED (no data!)\n";
    return false;
  }

  std::istringstream str(m_RecvBuf);
  int val;
  if ( !(str >> val) ) {
    std::cerr << "Serializer::getDIGIOstate FAILED (parse error)\n";
    return false;
  }

  busy = (val == 1);
  return true;  
}

bool 
Serializer::clearEncoders()
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::clearEncoders NOT CONNECTED\n";
    return false;
  }

  sendMsg("clrenc 1 2\r");
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::clearEncoders FAILED\n";
  return false;
}

bool 
Serializer::setMotorPWM(int motor, int pwm)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setMotorPWM NOT CONNECTED\n";
    return false;
  }

  char msg[128];

  if (motor != 0 && motor != 1) {
    std::cerr << "Serializer::setMotorPWM motor="
              << motor << " not supported, only 0 and 1\n";
    return false;
  }

  if (pwm < -100 || pwm > 100) {
    std::cerr << "Serialier::setMotorPWM pwm=" << pwm << " out of range [-100,100]\n";
    return false;
  }

  if (motor == 0) {
    sprintf(msg, "pwm 1:%d\r", pwm);
  } else {
    sprintf(msg, "pwm 2:%d\r", pwm);
  }
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setMotorPWM(" << motor 
            << ", " << pwm << ") FAILED\n";
  return false;
}

bool 
Serializer::setMotorPWMs(int pwm0, int pwm1)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setMotorPWMs NOT CONNECTED\n";
    return false;
  }

  if (pwm0 < -100 || pwm0 > 100) {
    std::cerr << "Serialier::setMotorPWM pwm0=" << pwm0 << " out of range [-100,100]\n";
    return false;
  }

  if (pwm1 < -100 || pwm1 > 100) {
    std::cerr << "Serialier::setMotorPWM pwm1=" << pwm1 << " out of range [-100,100]\n";
    return false;
  }

  char msg[128];
  sprintf(msg, "pwm 1:%d 2:%d\r", pwm0, pwm1);
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setMotorPWMs(" << pwm0 
            << ", " << pwm1 << ") FAILED\n";
  return false;
}

bool 
Serializer::setMotorSpeed(int motor, int speed)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setMotorSpeed NOT CONNECTED\n";
    return false;
  }

  char msg[128];

  if (motor != 0 && motor != 1) {
    std::cerr << "Serializer::setMotorSpeed motor="
              << motor << " not supported, only 0 and 1\n";
    return false;
  }

  if (motor == 0) {
    sprintf(msg, "mogo 1:%d\r", speed);
  } else {
    sprintf(msg, "mogo 2:%d\r", speed);
  }
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setMotorSpeed(" << motor 
            << ", " << speed << ") FAILED\n";
  return false;
}

bool 
Serializer::setMotorSpeeds(int speed0, int speed1)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setMotorSpeeds NOT CONNECTED\n";
    return false;
  }

  char msg[128];
  sprintf(msg, "mogo 1:%d 2:%d\r", speed0, speed1);
  sendMsg(msg);
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setMotorSpeeds(" << speed0 
            << ", " << speed1 << ") FAILED\n";
  return false;
}

bool
Serializer::getMotorSpeeds(int &speed0, int &speed1)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getMotorSpeeds NOT CONNECTED\n";
    return false;
  }

  sendMsg("vel\r");
  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getMotorSpeeds FAILED (no data)\n";
    return false;
  }

  std::istringstream str(m_RecvBuf);
  if ( !(str >> speed0 >> speed1) ) {
    std::cerr << "Serializer::getMotorSpeeds FAILED (parse error)\n";
    return false;
  }

  return true;
}

bool 
Serializer::stopAllMotors()
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::stopAllMotors NOT CONNECTED\n";
    return false;
  }

  sendMsg("stop\r");
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::stop FAILED\n";
  return false;
}

bool 
Serializer::readADC(std::vector< std::pair<int,int> > &query)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::readADC NOT CONNECTED\n";
    return false;
  }

  for (unsigned int i = 0; i < query.size(); i++) {
    if (query[i].first < 0 || query[i].first > 4) {
      std::cerr << "Serializer::readADC pin id must be 0-4, not "
                << query[i].first << std::endl;
      return false;
    }
  }
  std::ostringstream strOut;
  strOut << "sensor";
  for (unsigned int i = 0; i < query.size(); i++) {
    strOut << " " << query[i].first;
  }
  strOut << "\r";
  sendMsg(strOut.str().c_str());

  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::readADC FAILED (no data!)\n";
    return false;
  }

  std::istringstream strIn(m_RecvBuf);

  for (unsigned int i = 0; i < query.size(); i++) {

    if ( !(strIn >> query[i].second) ) {
      std::cerr << "Serializer::readADC FAILED (parse error)\n";
      return false;
    }

  }

  return true;
}

bool 
Serializer::setIO(std::vector< std::pair<int, bool> > &cmd)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setIO NOT CONNECTED\n";
    return false;
  }
  
  for (unsigned int i = 0; i < cmd.size(); i++) {
    // For now we only allow using the GPIO pins which cannot be used
    // for anything but IO and not to servos for example.
    if (cmd[i].first < 0 || cmd[i].first > m_MaxIO) {
      std::cerr << "Serializer::setIO pin id must be 0-"
		<< m_MaxIO << ", not "
                << cmd[i].first << std::endl;
      return false;
    }
  }
   
  std::ostringstream strOut;
  strOut << "setio";
  for (unsigned int i = 0; i < cmd.size(); i++) {
    strOut << " " << cmd[i].first << ":" << (int)cmd[i].second;
  }
  strOut << "\r";
  //std::cout << "Sending " << strOut.str() << std::endl;
  sendMsg(strOut.str().c_str());
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setIO FAILED\n";
  return false; 
}

bool
Serializer::getIO(std::vector< std::pair<int, bool> > &query)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::getIO NOT CONNECTED\n";
    return false;
  }
  
  for (unsigned int i = 0; i < query.size(); i++) {
    // For now we only allow using the GPIO pins which cannot be used
    // for anything but IO and not to servos for example.
    if (query[i].first < 0 || query[i].first > m_MaxIO) {
      std::cerr << "Serializer::getIO pin id must be 0-"
		<< m_MaxIO << ", not "
                << query[i].first << std::endl;
      return false;
    }
  }
   
  std::ostringstream strOut;
  strOut << "getio";
  for (unsigned int i = 0; i < query.size(); i++) {
    strOut << " " << query[i].first << ":" << (int)query[i].second;
  }
  strOut << "\r";
  sendMsg(strOut.str().c_str());

  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::getIO FAILED (no data!)\n";
    return false;
  }

  std::istringstream strIn(m_RecvBuf);

  for (unsigned int i = 0; i < query.size(); i++) {

    if ( !(strIn >> query[i].second) ) {
      std::cerr << "Serializer::getIO FAILED (parse error)\n";
      return false;
    }

  }

  return true; 
}

bool
Serializer::setServoPos(std::vector< std::pair<int, int> > &cmd)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::setServoPos NOT CONNECTED\n";
    return false;
  }

  for (unsigned int i = 0; i < cmd.size(); i++) {
    if (cmd[i].first < 4 || cmd[i].first > 9) {
      std::cerr << "Port for command element " << i << " is outside "
                << "allowed range 4-9, was " << cmd[i].first
                << std::endl;
      return false;
    }
  }

  std::ostringstream strOut;
  strOut << "servo";
  for (unsigned int i = 0; i < cmd.size(); i++) {
    strOut << " " << getServoPinIdFromPortNum(cmd[i].first) 
           << ":" << cmd[i].second;
  }
  strOut << "\r";
  sendMsg(strOut.str().c_str());
  if (waitForACK(m_ReplyTimeout)==0) return true;

  std::cerr << "Serializer::setServoPos FAILED\n";
  return false;
}

bool
Serializer::readSRF08(int &value, int address)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::readSRF08 NOT CONNECTED\n";
    return false;
  }

  char msg[64];
  sprintf(msg, "srf08 %d\r", address);
  sendMsg(msg);

  emptyRecvBuf();
  if (blockingRecvReply(m_ReplyTimeout)) {
    std::cerr << "Serializer::readSRF08(" << address << ") FAILED (no data)\n";
    return false;
  }

  std::istringstream str(m_RecvBuf);
  if ( !(str >> value) ) {
    std::cerr << "Serializer::readSRF08(" << address << ") FAILED (parse error)\n";
    return false;
  }

  return true;
}

int 
Serializer::waitSerialBuffer(double maxSecs)
{
  fd_set mask;
  struct timeval tv;

  tv.tv_sec = (int)maxSecs;
  tv.tv_usec = (long int)((maxSecs - tv.tv_sec)*1e6);

  FD_ZERO(&mask);
  FD_SET(m_Fd, &mask);

  int selret = select(FD_SETSIZE, &mask, NULL, NULL, &tv);
  if (selret > 0) {
    // Got data
    return 1;
  } else if (selret == 0) {
    return 0;
  } else {
    fprintf(stderr, "Error in wait_serial_buffer\n");
    perror("Select");
    return -1;
  }
}

int 
Serializer::readToRecvBuf()
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::readToRecvBuf NOT CONNECTED\n";
    return -1;
  }

  if (m_RecvBufPos == m_RecvBufSize) {
    fprintf(stderr, "Buffer overflow!!! Emptying buffer\n");
    m_RecvBufPos = 0;
  }
  
  int n =read(m_Fd, m_RecvBuf + m_RecvBufPos, m_RecvBufSize - m_RecvBufPos);
  
  if (n > 0) {
    m_RecvBufPos += n;
  } else if (n == 0) {
    fprintf(stderr, "Did not read any bytes\n");
  } else {
    fprintf(stderr, "Error reading serial device\n");
    perror("read");
  }  

  return n;
}

int 
Serializer::sendMsg(const char *msg)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::sendMsg NOT CONNECTED\n";
    return -1;
  }

  int len = strlen(msg);

  if (msg[len-1] != '\r') {
    std::cerr << "Serializer::sendMsg Message not terminated by CR1!. It will fail\n";
  }

  if (write(m_Fd, (void *)msg, len) != len) {
    std::cerr << "Serializer::sendMsg Could not send message \""
	      << msg << "\"\n";
    return -1;
  }

  /*
  fprintf(stderr, "Serializer::sendMsg Sent : ");
  for (int i = 0; i < len; i++) fprintf(stderr, "%02X ", (int)msg[i]);
  fprintf(stderr, " (\"%s\"\n", msg);
  */

  return 0;
}

int 
Serializer::blockingRecvReply(double maxSecs)
{
  if (m_Fd <= 0) {
    std::cerr << "Serializer::blockinRecvReply NOT CONNECTED\n";
    return -1;
  }

  double t0 = getCurrentTime();
  double tLeft = maxSecs;

  // Hang here until we get the reply or too much time has passed
  do {
    
    int ret = waitSerialBuffer(tLeft);
    if (ret == 1) {
      
      readToRecvBuf();
      
      // Each message ends with CR + LF + '>' which means that even if
      // the data part of the message we get is only one char long we
      // need to get at least 4 chars
      if (m_RecvBufPos < 4) {
	//std::cerr << "Serializer::blockingRecvReply not enough data yet (" << m_RecvBufPos << ")\n";
	continue;
      }

      // Make printing of recv buffer nicer by terminating the string
      if (m_RecvBufPos < m_RecvBufSize) m_RecvBuf[m_RecvBufPos] = 0; 

      //fprintf(stderr, "Received %d bytes\n", m_RecvBufPos);
      /*
      fprintf(stderr, "Got %d bytes: ", m_RecvBufPos);
      for (int i = 0; i < m_RecvBufPos; i++) 
	fprintf(stderr, "%02X ", (int)m_RecvBuf[i]);
      fprintf(stderr, " (\"%s\")\n", m_RecvBuf);
      */

    } else if (ret == 0) {
      
      // Got timed out
      std::cerr << "Serializer::blockingRecvReply timed out "
		<< maxSecs << std::endl;
      return -1;

    } else {

      // Got error
      std::cerr << "Serializer::blockingRecvReply ERROR getting reply\n";
      return -2;

    }
    
    // Check if we got the whole message, i.e. got the "\r\n>" ending
    // (we thus do not need to go all the way to the end as we are
    // looking for a 3 character ending
    for (int i = 0; i < m_RecvBufPos-2; i++) {

      if (m_RecvBuf[i] == '\r') {

	if (m_RecvBuf[i+1] == '\n' &&
	    m_RecvBuf[i+2] == '>') {
	
	  //std::cerr << "Serializer::blockingRecvReply Found msg end\n";
	  
	  // We do not need to process the end
	  m_RecvBuf[i] = 0;
	  m_RecvBufPos -= 3;
	  
	  /*
	  fprintf(stderr, "Got the reply: ");
	  for (int j = 0; j < m_RecvBufPos; j++) 
	    fprintf(stderr, "%02X(%c) ", (int)m_RecvBuf[j],m_RecvBuf[j]);
	  fprintf(stderr, " dt=%.6fs\n", getCurrentTime()-t0);
	  */
	  return 0;

	} else {
	  //std::cerr << "Serializer::blockingRecvReply Got non-end CR at i=" << i << std::endl;

	  // We skip this non-end CR
	  memmove(m_RecvBuf+i, m_RecvBuf+i+1, m_RecvBufPos-i-1);
	  i--;
	  m_RecvBufPos--;
	}

      } else if (m_RecvBuf[i] == '\n') {
	//std::cerr << "Serializer::blockingRecvReply Got LF at i=" << i << std::endl;

	// We skip LFs
	memmove(m_RecvBuf+i, m_RecvBuf+i+1, m_RecvBufPos-i-1);
	i--;
	m_RecvBufPos--;
      }

    }

    tLeft = maxSecs - (getCurrentTime() - t0);
    if (tLeft < 0) {
      std::cerr << "Serializer::blockingRecvReply Got timeout\n";
      return -1;
    }
    
  } while (true);

  // Should never get here
  std::cerr << "Serializer::blockingRecvReply Should never get to the end of blocking_recv\n";
  return -3;
}

int 
Serializer::waitForACK(double maxSecs)
{
  emptyRecvBuf();  
  if (blockingRecvReply(maxSecs) == 0) {

    //std::cerr << "Serializer::waitForACK Got \"" << m_RecvBuf << "\"\n";

    if (strncmp(m_RecvBuf, "ACK", 3)==0) {
      //std::cout << "Serializer::waitForACK Got the ACK\n";
      return 0;
    }
  }

  std::cout << "Serializer::waitForACK Did not get the ACK\n";
  return -1;
}

int 
Serializer::handshake()
{
  char msg[] = "blink 1:127\r";
  
  // Send the msg. If we got an ACK that we could parse we are
  // good. We try a few times, then give up
  for (int i = 0; i < 3; i++) {
    if (sendMsg(msg)) return -1;
    if (waitForACK(0.5)==0) return 0;
  }
  
  // If we get here it means that we failed 
  std::cerr << "Serializer::handshake() FAILED\n";
  return -1;
}

int 
Serializer::setTermios(int baudrate)
{
  struct termios info;

  info.c_iflag = 0;
  info.c_oflag = 0;
  info.c_lflag = 0;

  switch (baudrate ) { 
  case 2400:
    info.c_cflag = B2400 | CS8 | CREAD | CLOCAL;
    break;
  case 4800:
    info.c_cflag = B4800 | CS8 | CREAD | CLOCAL;
    break;
  case 9600:
    info.c_cflag = B9600 | CS8 | CREAD | CLOCAL;
    break;
  case 19200:
    info.c_cflag = B19200 | CS8 | CREAD | CLOCAL;
    break;
  case 38400:
    info.c_cflag = B38400 | CS8 | CREAD | CLOCAL;
    break;
  case 115200:
    info.c_cflag = B115200 | CS8 | CREAD | CLOCAL;
    break;
  default:
    fprintf(stderr, "Ivalid baudrate %d, using 19200\n", baudrate);
    info.c_cflag = B19200 | CS8 | CREAD | CLOCAL;
    break;
  }

  if (tcsetattr(m_Fd, TCSANOW, &info) < 0) { 
    perror("Error using TCSETS in ioctl ");
    return -1;
  }

  return 0;
}

int
Serializer::getServoPinIdFromPortNum(int num)
{
  switch (num) {
    case 4:
      return 5;
    case 5:
      return 6;
    case 6:
      return 3;
    case 7:
      return 4;
    case 8:
      return 1;
    case 9:
      return 2;
    default:
      std::cerr << "Serializer::getServoPinIdFromPortNum("
                << num << ") input out of range\n";
      return -1;
  }
}

int
Serializer::getServoPortNumFromPinId(int id)
{
  switch (id) {
    case 1:
      return 8;
    case 2:
      return 9;
    case 3:
      return 6;
    case 4:
      return 7;
    case 5:
      return 4;
    case 6:
      return 5;
    default:
      std::cerr << "Serializer::getServoPortNumFromPinId("
                << id << ") input out of range\n";
      return -1;
  }
}

